#include <string.h>
#include "vehicle_status.h"
#include "indicators.h"

VehicleStatus::~VehicleStatus()
{
  quit_mutex_.unlock();
	tid_.join();
}

void VehicleStatus::Batterys_Callback(const agv_msgs::msg::BatInfo::ConstSharedPtr  bat_msg)
{
  if ( scenesPtr_->size() <= CHARGING)
  {
    DEBUG_ERROR_OUT("[Batterys_Callback] wrong scenes size=" << scenesPtr_->size());
    return;
  }
  scenesPtr_->at(CHARGING).trigSet( bat_msg->charge_flag);
}

void VehicleStatus::agvmodeCallback(const agv_msgs::msg::AGVMode::ConstSharedPtr state_p)
{
	mode_ = *state_p;
  if ( scenesPtr_->size() <= MANUAL)
  {
    DEBUG_ERROR_OUT("[agvmodeCallback] wrong scenes size=" << scenesPtr_->size());
    return;
  }
  scenesPtr_->at(MANUAL).trigSet( mode_.mode == agv_msgs::msg::AGVMode::MANUAL);
  scenesPtr_->at(SEMI_AUTO).trigSet( mode_.mode == agv_msgs::msg::AGVMode::SEMI_AUTOMATIC);
}

// 是否是自旋
bool VehicleStatus::spinRun()
{
  if (!useSpinAlarm_)
  {
    return false;
  }
  bool nav = navRun();
  bool spin = autoStatus_.trajtype == agv_msgs::msg::AgvAutoStatus::TRAJ_SPIN;
  return nav && spin;
}

// 是否时运行时后退
bool VehicleStatus::backward()
{
    bool ret = false;
    if (!useBackAlarm_)
    {
        return ret;
    }

    bool back = agv_msgs::msg::AgvAutoStatus::BACKWARD_MOVE == autoStatus_. wheelmovedir 
            && (agv_msgs::msg::AgvAutoStatus::TRAJ_LINE == autoStatus_.trajtype || 
              agv_msgs::msg::AgvAutoStatus::TRAJ_BEZIER == autoStatus_.trajtype);
    bool drive = autoStatus_.driving;
    ret = back && drive && navRun();
    // ROS_INFO_STREAM("backward=" << ret << ",back=" << back << ",drive=" << drive);
    return ret;
}

// 是否是导航运行
bool VehicleStatus::navRun()
{
    bool nav = agv_msgs::msg::AgvAutoStatus::IS_NAVIGATE == autoStatus_.workstate;
    bool idle = autoStatus_.idle;
    return nav && !idle && mode_.mode == agv_msgs::msg::AGVMode::AUTOMATIC;
}

void VehicleStatus::AutoStatus_Callback(const agv_msgs::msg::AgvAutoStatus::ConstSharedPtr  autoStatus_msg)
{
  autoStatus_= *autoStatus_msg;
}

void VehicleStatus::tapeSensor_Callback(const agv_msgs::msg::TapeSensors::ConstSharedPtr pMsg)
{
  tape_.Update(*pMsg);
}

void VehicleStatus::readInputCallback(const agv_msgs::msg::ReadInPutsNew::ConstSharedPtr &msg)
{
  std::vector<agv_msgs::msg::PairTypeInt> inputs = msg->array;
  for (auto &data : inputs)
  {
    if (data.key == BUMPERSHIELD_STATE)
    {
      protection_bypassed_ = data.value;
    }                 
  }
}


void VehicleStatus::readOutputCallback(const agv_msgs::msg::OutPutsNew::ConstSharedPtr &msg)
{
  std::vector<agv_msgs::msg::PairTypeInt> outputs = msg->array;
  for (auto &data : outputs)
  {
    if (data.key == BUMPERSHIELD_RELAY)
    {
        protection_bypass_ = data.value;
    }
          
  }
}

void VehicleStatus::eventCheck(int ecode, EventTrig &trigs)
{
  switch (ecode)
  {
  case agv_msgs::msg::AGVEvent::RUN_STOP_EVENT:
    trigs.temp_parking= true;
    break;

  case agv_msgs::msg::AGVEvent::BATTERY_LOW_EVENT:
    trigs.low_battery= true;
    break;

  case agv_msgs::msg::AGVEvent::SAFE_DETECT_CENTER_EVENT:
  case agv_msgs::msg::AGVEvent::SAFE_DETECT_FAR_EVENT:
    trigs.safety_scanner_slowDown= true;
    break;    

  case agv_msgs::msg::AGVEvent::HOST_EVENT:
  case agv_msgs::msg::AGVEvent::NETWORK_FAIL_EVENT:
    trigs.lost_host= true;
    break;  

  case agv_msgs::msg::AGVEvent::BATTERY_TOO_LOW_EVENT:
    trigs.too_low_battery= true;
    break;  

  case agv_msgs::msg::AGVEvent::PATH_GUIDE_LOST_EVENT:
    trigs.off_path= true;
    break;  

  case agv_msgs::msg::AGVEvent::TASK_ERROR_EVENT:
    trigs.task_cancel= true;
    break;  

  case agv_msgs::msg::AGVEvent::LOAD_AND_UNLOAD_ERROR:
    trigs.loader_action_failed= true;
    break;  

  case agv_msgs::msg::AGVEvent::HMI_BUTTON_EVENT:
  case agv_msgs::msg::AGVEvent::SAFETY_PAUSE:
    trigs.vehicle_safety_ack= true;
    break;  

  case agv_msgs::msg::AGVEvent::LOCATION_MISSING_EVENT:
  case agv_msgs::msg::AGVEvent::MARK_SENSOR_ERROR_EVENT:
  case agv_msgs::msg::AGVEvent::GUIDER_SENSOR_ERR_EVENT:
  case agv_msgs::msg::AGVEvent::SLAM_LIDAR_DATA_ERROR:
  case agv_msgs::msg::AGVEvent::SLAM_LIDAR_DATA_WARNING:
  case agv_msgs::msg::AGVEvent::SLAM_ODOM_DATA_ERROR:  
  case agv_msgs::msg::AGVEvent::SLAM_STATE_ERROR:
  case agv_msgs::msg::AGVEvent::MAGNET_INIT_ERROR:
  case agv_msgs::msg::AGVEvent::MAGNET_MAGNET_MARK_ERROR:
  case agv_msgs::msg::AGVEvent::MAGNET_DATE_ERROR:
  case agv_msgs::msg::AGVEvent::MAGNET_PARAM_ERROR:
  case agv_msgs::msg::AGVEvent::QRCODE_MISS_ERROR:
  case agv_msgs::msg::AGVEvent::QRCODE_MISS_WARNING:   
    trigs.location= true;
    break;  

  case agv_msgs::msg::AGVEvent::STEER_REACH_LIMIT_EVENT: 
  case agv_msgs::msg::AGVEvent::CAN_EVENT: 
  case agv_msgs::msg::AGVEvent::EQUIPMENT_EVENT: 
  case agv_msgs::msg::AGVEvent::BATTERY_ERROR_EVENT: 
  case agv_msgs::msg::AGVEvent::CANIO_ERROR_EVENT: 
  case agv_msgs::msg::AGVEvent::CANOPEN_SERVO_ERROR_EVENT: 
  case agv_msgs::msg::AGVEvent::QR_READER_SENSOR_ERROR: 
  case agv_msgs::msg::AGVEvent::IO_TIME_OUT: 
  case agv_msgs::msg::AGVEvent::OBSTACLE_AVOID_TIME_OUT: 
  case agv_msgs::msg::AGVEvent::EVENT_TIME_OUT: 
  case agv_msgs::msg::AGVEvent::TOPIC_TIME_OUT: 
  case agv_msgs::msg::AGVEvent::TOPIC_LOW_FREQ: 
  case agv_msgs::msg::AGVEvent::OBSTACLE_AVOID_DATA_ERROR: 
  case agv_msgs::msg::AGVEvent::FORK_LIFT_UPLIMIT: 
  case agv_msgs::msg::AGVEvent::FORK_LIFT_DOWNLIMIT: 
  case agv_msgs::msg::AGVEvent::FORK_LIFT_TIMEOUT: 
  case agv_msgs::msg::AGVEvent::ACTION_NEEDCLEAR_EVENT: 
  case agv_msgs::msg::AGVEvent::CHARGEPLATE_WRONG_PLACE: 
  case agv_msgs::msg::AGVEvent::PROCESS_EXECUTION_FAILED: 
  case agv_msgs::msg::AGVEvent::CPU_OVERLOAD_EVENT: 
  case agv_msgs::msg::AGVEvent::MEM_OVERLOAD_EVENT: 
  case agv_msgs::msg::AGVEvent::DISK_OVERLOAD_EVENT: 
  case agv_msgs::msg::AGVEvent::CHARGE_FAIL_EVENT:
    trigs.vehicle_fault= true;                  
    break;                   

  case agv_msgs::msg::AGVEvent::SAFE_DETECT_NEAR_EVENT: 
  case agv_msgs::msg::AGVEvent::FORK_TIP_EVENT: 
    trigs.safety_scanner_stop= true;                  
    break;                   

  case agv_msgs::msg::AGVEvent::BUMPER_EVENT: 
    trigs.collision_sensor= true;                  
    break;         

  case agv_msgs::msg::AGVEvent::EMG_STOP_EVENT: 
    trigs.estop_button= true;                  
    break;                   

  default:
    break;
  }    
}

EventTrig VehicleStatus::getTrigs(void)
{
  std::lock_guard<std::mutex> locker( mutex_);
  return trigs_;
}

void VehicleStatus::eventPolling(void)
{
  std::lock_guard<std::mutex> locker( mutex_);
  trigs_.reset();
  if ( scenesPtr_->size() <= ESTOP_BUTTON)
  {
    DEBUG_ERROR_OUT("[Batterys_Callback] wrong scenes size=" << scenesPtr_->size());
    return;

  }
  // DEBUG_OUT("[eventCheck] events num:" << agv_event_.events.size());        
  for (auto event : agv_event_.events)
  {
    eventCheck( event.ecode, trigs_);
  }
  eventCheck( agv_event_.ecode, trigs_);

  scenesPtr_->at(TEMP_PARKING).trigSet( trigs_.temp_parking);
  scenesPtr_->at(LOW_BATTERY).trigSet( trigs_.low_battery);
  scenesPtr_->at(SAFETY_SCANNER_SLOWDOWN).trigSet( trigs_.safety_scanner_slowDown);
  scenesPtr_->at(LOST_HOST).trigSet( trigs_.lost_host);
  scenesPtr_->at(TOO_LOW_BATTERY).trigSet( trigs_.too_low_battery);
  scenesPtr_->at(OFF_PATH).trigSet( trigs_.off_path);
  scenesPtr_->at(TASK_CANCEL).trigSet( trigs_.task_cancel);
  scenesPtr_->at(LOADER_ACTION_FAILED).trigSet( trigs_.loader_action_failed);  
  scenesPtr_->at(VEHICLE_SAFETY_ACK).trigSet( trigs_.vehicle_safety_ack);
  scenesPtr_->at(NAVIGATION_FAILED).trigSet( trigs_.location);
  scenesPtr_->at(VEHICLE_FAULT).trigSet( trigs_.vehicle_fault);
  scenesPtr_->at(SAFETY_SCANNER_STOP).trigSet( trigs_.safety_scanner_stop);
  scenesPtr_->at(COLLISION_SENSOR).trigSet( trigs_.collision_sensor);
  scenesPtr_->at(ESTOP_BUTTON).trigSet( trigs_.estop_button);
  // DEBUG_OUT("[eventCheck] Vehicle event update over");    
}

void VehicleStatus::event_Callback(const agv_msgs::msg::AGVEventStatus::ConstSharedPtr &event)
{
  agv_event_= *event;
}

void VehicleStatus::setStopCallback(const agv_msgs::msg::SetStopBits::ConstSharedPtr &msg)
{
  // std::lock_guard<std::mutex> locker(m_mutex_);

  // if (pMB_Master_ == nullptr)
  // {
  //   RCLCPP_ERROR(rclcpp::get_logger("AgvIndicators"), "error:pMB_Master_ is nullptr");
  //   return;
  // }

  // pMB_Master_->modbusServer_set_Word16register( StopWord_addr, msg->stop);
}

void VehicleStatus::landmarkCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr &msg)
{
  qrSensorDetect_.Update(true);
}

bool VehicleStatus::networkCheck(void)
{
  bool disconnection= netMsg_.brokerconnnection || netMsg_.masterconnnection;
    // #2 网络在线输出灯
    return !disconnection;
}

void VehicleStatus::networkCB(const agv_msgs::msg::ConnectionMonitor::ConstSharedPtr &msg)
{
  netMsg_ = *msg;
}

void VehicleStatus::naviTypeCB(const agv_msgs::msg::NavType::ConstSharedPtr &msg)
{
  naviType_ = *msg;
}

bool VehicleStatus::alarmCheck(void)
{
  // #2 获取车辆报警状态
  return (agv_event_.type == agv_msgs::msg::AGVEvent::ALARM);
}

void VehicleStatus::runstateCheck(void)
{
  if ( scenesPtr_->size() <= ESTOP_BUTTON)
  {
    DEBUG_ERROR_OUT("[AutoStatus_Callback] wrong scenes size=" << scenesPtr_->size());
    return;
  }
  scenesPtr_->at(IDLE).trigSet( autoStatus_.idle);
  scenesPtr_->at(DRIVING).trigSet( navRun());	
  scenesPtr_->at(ROTATION).trigSet( spinRun());	
  scenesPtr_->at(BACKWARD).trigSet( backward());                                 
  scenesPtr_->at(PAUSED).trigSet( autoStatus_.paused);
  scenesPtr_->at(START_DELAY).trigSet( autoStatus_.startdelay);

  if ( danger_bitIndex_ >= 0 && danger_bitIndex_ < 16)
    scenesPtr_->at(DANGER_AREA).trigSet( TestBit( autoStatus_.bits, danger_bitIndex_));
}

void VehicleStatus::workingCheck(void)
{
  std::lock_guard<std::mutex> locker( vdamutex_);
  bool working(false);

  for (auto &action : vda_.actionstates)
  {
    if (action.actionstatus.compare("RUNNING") != 0)
      continue;
    
    if (action.actiontype.compare("pick") == 0) {
      working= true;  
    } 
    else if(action.actiontype.compare("drop") == 0){
      working= true;   
    }
    else
    {
    }
  }
  scenesPtr_->at(WORKING).trigSet( working);
  // DEBUG_OUT("[workingCheck]working:" << working);
}

void VehicleStatus::vda5050Callback(const agv_msgs::msg::State::ConstSharedPtr &msg)
{
  std::lock_guard<std::mutex> locker( vdamutex_);
  vda_ = *msg;
}

void VehicleStatus::poseCB(const agv_msgs::msg::PoseWithConfidence::ConstSharedPtr &msg)
{
    poseMsg_ = *msg;
}


bool VehicleStatus::poseCheck(void)
{
  // #4 置信度高于15成功
    return poseMsg_.score > 15.0;
}

void VehicleStatus::cmd_velCallBack(const geometry_msgs::msg::TwistStamped::ConstSharedPtr &msg)
{
  bool left = (msg->twist.angular.z > 0.15) && navRun();
  bool right = msg->twist.angular.z < -0.15 && navRun();

  scenesPtr_->at(TURN_LEFT).trigSet( left);
  scenesPtr_->at(TURN_RIGHT).trigSet( right);
}

void VehicleStatus::eventPub(int index, agv_msgs::msg::AGVEvent *event)
{

}

void VehicleStatus::protection_bypassCheck(void)
{
  scenesPtr_->at(SAFETY_SENSOR_BYPASS).trigSet( protection_bypass_ || protection_bypassed_);
}

//事件分类至场景
void VehicleStatus::eventSort2Scene(void)
{
  scenesPtr_->at(SAFETY_SENSOR_BYPASS).trigSet( protection_bypass_ || protection_bypassed_);
}

bool VehicleStatus::qrCodeValidCheck(void)
{
  if ( qrSensorDetect_.TimeOut(800))
  {
    qrSensorDetect_.Update(false);
  }
  
  
  bool trigged = mode_.mode == agv_msgs::msg::AGVMode::MANUAL && qrSensorDetect_.IsNew(800) && qrSensorDetect_.Get();
  scenesPtr_->at(QR_FOUND).trigSet( trigged);
  return trigged;
}

// 磁带检测到超过两条，认为在磁条上  
bool VehicleStatus::tapeValidCheck(void)
{
  bool trigged(false);
  int tape= 0;
  for (auto sensor : tape_.Get().sensors)
  {
    if ( sensor.valid) tape++;
  }
  trigged= mode_.mode == agv_msgs::msg::AGVMode::MANUAL && tape_.IsNew(800) && (tape >= 2);
  scenesPtr_->at(TAPE_FOUND2).trigSet( trigged);
  return trigged;
}

void VehicleStatus::poll_looper(void *arg)
{
  VehicleStatus* pThis = (VehicleStatus*)arg;   
  while ( !pThis->quit_mutex_.try_lock_for(std::chrono::milliseconds(20)))
  {
    pThis->qrCodeValidCheck();
    pThis->tapeValidCheck();
    pThis->protection_bypassCheck();
    pThis->eventPolling();
    pThis->workingCheck();
    pThis->runstateCheck();
  }
  RCLCPP_WARN(rclcpp::get_logger("AgvIndicators"), "[poll_looper] thread exit"); 
  return;
}
int VehicleStatus::init(const std::shared_ptr<rclcpp::Node> node)
{
    node_ = node;
	  //4. init ros node, 
    node_->get_parameter_or<bool>("use_back_alarm", useBackAlarm_, true);      
    node_->get_parameter_or<bool>("use_spin_alarm", useSpinAlarm_, true);
    node_->get_parameter_or<int>("danger_bit_index", danger_bitIndex_, -1);
    battery_sub_ = node_->create_subscription<agv_msgs::msg::BatInfo>(
        "batteryInfo",
        rclcpp::QoS(1),
        [this](const agv_msgs::msg::BatInfo::ConstSharedPtr msg) {
            Batterys_Callback(msg);
        });

    agvmode_sub_ = node_->create_subscription<agv_msgs::msg::AGVMode>(
        "/agvmode",
        rclcpp::QoS(1),
        [this](const agv_msgs::msg::AGVMode::ConstSharedPtr msg) {
            agvmodeCallback(msg);
        });

    autoStatus_sub_ = node_->create_subscription<agv_msgs::msg::AgvAutoStatus>(
        "auto_run_status",
        rclcpp::QoS(1),
        [this](const agv_msgs::msg::AgvAutoStatus::ConstSharedPtr msg) {
            AutoStatus_Callback(msg);
        });

    location_sub_ = node_->create_subscription<agv_msgs::msg::PoseWithConfidence>(
        "/pose",
        rclcpp::QoS(2),
        [this](const agv_msgs::msg::PoseWithConfidence::ConstSharedPtr msg) {
            poseCB(msg);
        });

    eventOccurSub_ = node_->create_subscription<agv_msgs::msg::AGVEventStatus>(
        "/eventstatus",
        rclcpp::QoS(1),
        [this](const agv_msgs::msg::AGVEventStatus::ConstSharedPtr msg) {
            event_Callback(msg);
        });

    tapeSensorSub_ = node_->create_subscription<agv_msgs::msg::TapeSensors>(
        "/base/tape_sensor",
        rclcpp::QoS(1),
        [this](const agv_msgs::msg::TapeSensors::ConstSharedPtr msg) {
            tapeSensor_Callback(msg);
        });

    readinput_sub_ = node_->create_subscription<agv_msgs::msg::ReadInPutsNew>(
        "/readInputNew",
        rclcpp::QoS(10),
        [this](const agv_msgs::msg::ReadInPutsNew::ConstSharedPtr msg) {
            readInputCallback(msg);
        });

    readoutput_sub_ = node_->create_subscription<agv_msgs::msg::OutPutsNew>(
        "/outputNew",
        rclcpp::QoS(10),
        [this](const agv_msgs::msg::OutPutsNew::ConstSharedPtr msg) {
            readOutputCallback(msg);
        });

    set_stop_sub_ = node_->create_subscription<agv_msgs::msg::SetStopBits>(
        "/set_stop_bits",
        rclcpp::QoS(10),
        [this](const agv_msgs::msg::SetStopBits::ConstSharedPtr msg) {
            setStopCallback(msg);
        });

    landmarkSub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
        "/landmark",
        rclcpp::QoS(10),
        [this](const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) {
            landmarkCallback(msg);
        });

    vda5050_sub_ = node_->create_subscription<agv_msgs::msg::State>(
        "/vda5050_state",
        rclcpp::QoS(1),
        [this](const agv_msgs::msg::State::ConstSharedPtr msg) {
            vda5050Callback(msg);
        });

    network_connection_sub_ = node_->create_subscription<agv_msgs::msg::ConnectionMonitor>(
        "connection_state",
        rclcpp::QoS(1),
        [this](const agv_msgs::msg::ConnectionMonitor::ConstSharedPtr msg) {
            networkCB(msg);
        });

    navi_type_sub_ = node_->create_subscription<agv_msgs::msg::NavType>(
        "/navigation_type",
        rclcpp::QoS(1),
        [this](const agv_msgs::msg::NavType::ConstSharedPtr msg) {
            naviTypeCB(msg);
        });

    odom_sub_ = node_->create_subscription<geometry_msgs::msg::TwistStamped>(
        "/cmd_vel",
        rclcpp::QoS(2),
        [this](const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) {
            cmd_velCallBack(msg);
        });
    quit_mutex_.lock();
    tid_ = std::thread(poll_looper, this);
    DEBUG_OUT("[VehicleStatus] init success,create poll_looper thread");
    return 0;
}

